//
// Created by JYL on 2024-06-17.
//
#include "filter_task.h"
#include "balance_task.h"
#include "cmsis_os.h"
#include "mpu6050.h"
#include "IOI2C.h"
#include "KF.h"
#include "filter.h"

short Accel_Y,Accel_Z,Accel_X,Gyro_X,Gyro_Z,Gyro_Y;
MPU6050 mpu6050;
extern osSemaphoreId_t myBinarySem_mpuHandle;
extern osSemaphoreId_t myBinarySem_carHandle;
/*
 * 根据传感器数据进行的操作
 * 1.利用陀螺仪mpu6050完成姿态解算，得出欧拉角
 * 操作完之后释放信号量，通知平衡任务
 * */
void Filter_Task(void *argument)
{
    /* USER CODE BEGIN Filter_Task */
    balance.Way_Angle=1;        //DMP库提取的角速度数据，跳变范围很大，不建议使用s
    /* Infinite loop */
    for(;;)
    {//更新姿态，5ms一次，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
        if(osSemaphoreAcquire(myBinarySem_mpuHandle,portMAX_DELAY)==osOK){
            Get_Angle(balance.Way_Angle);
            osDelay(5);
        }
    }
    /* USER CODE END Filter_Task */
}



///**************************************************************************
//函数功能：获取角度
//入口参数：way：获取角度的算法 1：DMP  2：卡尔曼 3：互补滤波
//返回  值：无
//**************************************************************************/
void Get_Angle(uint8_t way)
{
    float gyro_x,gyro_y,accel_x,accel_y,accel_z;
    //Temperature=Read_Temperature();      //读取MPU6050内置温度传感器数据，近似表示主板温度。
    if(way==0)                           //DMP的读取在数据采集中断读取，严格遵循时序要求
    {
        Read_DMP();                      	 //读取加速度、角速度、倾角
        mpu6050.Angle_Balance=Pitch;             	 //更新平衡倾角,前倾为正，后倾为负
        mpu6050.Gyro_Balance=gyro[0];              //更新平衡角速度,前倾为正，后倾为负
        mpu6050.Gyro_Turn=gyro[2];                 //更新转向角速度
        mpu6050.Acceleration_Z=accel[2];           //更新Z轴加速度计
    }
    else
    {
        Gyro_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);    //读取X轴陀螺仪
        Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //读取Y轴陀螺仪
        Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //读取Z轴陀螺仪
        Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计
        Accel_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //读取X轴加速度计
        Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计
        mpu6050.Gyro_Balance=-Gyro_X;                            //更新平衡角速度
        accel_x=Accel_X/1671.84;
        accel_y=Accel_Y/1671.84;
        accel_z=Accel_Z/1671.84;
        gyro_x=Gyro_X/939.8;                              //陀螺仪量程转换
        gyro_y=Gyro_Y/939.8;                              //陀螺仪量程转换
        if(way==1)
        {
            Pitch= KF_X(accel_y,accel_z,-gyro_x)/PI*180;//卡尔曼滤波
            Roll = KF_Y(accel_x,accel_z,gyro_y)/PI*180;
        }
        else if(way==2)
        {
            Pitch = -Complementary_Filter_x(accel_x,gyro_x);//互补滤波
            Roll = -Complementary_Filter_y(accel_y,gyro_y);
        }
        mpu6050.Angle_Balance=Pitch;                              //更新平衡倾角
        mpu6050.Gyro_Turn=Gyro_Z;                                 //更新转向角速度
        mpu6050.Acceleration_Z=Accel_Z;                           //更新Z轴加速度计
    }
}

